﻿using System;

namespace Silver3D.Math
{
    public struct Quat
    {
        public static Quat Zero { get { return new Quat(Vec3.Zero, 0.0); } }
        public static Quat Identity { get { return new Quat(Vec3.Zero, 1.0); } }

        public Vec3 v;
        public double s;

        public double LengthSquared
        {
            get { return v.Dot(v) + s * s; }
        }

        public double Length
        {
            get
            {
                double lsq = LengthSquared;
                if (lsq == 0.0) return 0.0;
                return System.Math.Sqrt(lsq);
            }
        }

        public Quat Unit
        {
            get { return this / Length; }
        }

        public Quat Conjugate
        {
            get { return new Quat(-v, s); }
        }

        public Quat(Vec3 v)
        {
            this.v = v;
            s = 0.0;
        }

        public Quat(Vec3 v, double s)
        {
            this.v = v;
            this.s = s;
        }

        public Quat(Quat q)
        {
            v = q.v;
            s = q.s;
        }

        public static Quat operator *(Quat lhs, Quat rhs)
        {
            return new Quat(lhs.v.Cross(rhs.v) + (lhs.s * rhs.v) + (lhs.v * rhs.s),
                lhs.s * rhs.s - lhs.v.Dot(rhs.v));
        }

        public static Quat operator *(double lhs, Quat rhs)
        {
            return new Quat(rhs.v * lhs, rhs.s * lhs);
        }

        public static Quat operator *(Quat lhs, double rhs)
        {
            return new Quat(rhs * lhs.v, rhs * lhs.s);
        }

        public static Quat operator /(Quat lhs, double rhs)
        {
            return lhs * (1.0 / rhs);
        }

        public static bool operator ==(Quat lhs, Quat rhs)
        {
            return (lhs.v == rhs.v) && (lhs.s == rhs.s);
        }

        public static bool operator !=(Quat lhs, Quat rhs)
        {
            return (lhs.v != rhs.v) || (lhs.s != rhs.s);
        }

        public override string ToString()
        {
            return string.Format("Quat(({0},{1},{2}),{3})", v.X, v.Y, v.Z, s);
        }

        public bool IsEquivalent(Quat q)
        {
            if (q == null) return false;
            return v.IsEquivalent(q.v) && Util.IsEquivalent(s, q.s);
        }

        public override bool Equals(object obj)
        {
            return (obj != null) && (obj is Quat) && ((Quat)obj) == this;
        }

        public override int GetHashCode()
        {
            return v.GetHashCode() ^ s.GetHashCode();
        }

        public void ToAngleAxis(out double angle, out Vec3 axis)
        {
            angle = 2.0 * System.Math.Acos(s);
            double sz = System.Math.Sin(angle * 0.5);
            if (!Util.IsEquivalent(sz, 0.0))
            {
                axis = (v / sz).Unit;
            }
            else
            {
                axis = Vec3.ZAxis;
            }
        }

        public Matrix44 ToMatrix()
        {
            // adapted from: 
            // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm

            double scalar, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz, den;

            if (IsEquivalent(Quat.Identity)) 
            {
                return Matrix44.Identity;
            }

            den = LengthSquared;
            if (Util.IsEquivalent(den, 0.0)) scalar = 1.0;
            else scalar = 2.0 / den;

            xs = v.X * scalar; ys = v.Y * scalar; zs = v.Z * scalar;
            wx = s * xs;  wy = s * ys; wz = s * zs;
            xx = v.X * xs;  xy = v.X * ys; xz = v.X * zs;
            yy = v.Y * ys;  yz = v.Y * zs; zz = v.Z * zs;

            return new Matrix44(
                1.0 - yy - zz, xy - wz, xz + wy, 0.0,
                xy + wz, 1.0 - xx - zz, yz - wx, 0.0,
                xz - wy, yz + wx, 1.0 - xx - yy, 0.0,
                0.0, 0.0, 0.0, 1.0);
        }
    }
}
